/**
 *             _..--""-.                  .-""--.._
           _.-'         \ __...----...__ /         '-._
         .'      .:::...,'              ',...:::.      '.
        (     .'``'''::;                  ;::'''``'.     )
         \             '-)              (-'             /
          \             /                \             /
           \          .'.-.            .-.'.          /
            \         | \0|            |0/ |         /
            |          \  |   .-==-.   |  /          |
             \          `/`;          ;`\`          /
              '.._      (_ |  .-==-.  | _)      _..'
                  `"`"-`/ `/'        '\` \`-"`"`
                       / /`;   .==.   ;`\ \
                 .---./_/   \  .==.  /   \ \
                / '.    `-.__)       |    `"
               | =(`-.        '==.   ;
                \  '. `-.           /
                 \_:_)   `"--.....-'

                 file: gripper_publisher.cpp
                 description: state publisher for the urdf gripper.
                 authors: Michiel and Peter
 */

#include "gripper/gripper_publisher.h"

/**
 * Sensor callback.
 * @param	msg	distance from sensor in cm.
 */
void GripperPublisher::sensorCallback(const std_msgs::Int32::ConstPtr &msg)
{
	distance = msg->data / 10.0;	// convert to dm
}

/**
 * Motor callback.
 * @param	state Int32, OPEN or CLOSED.
 */
void GripperPublisher::motorCallback(const std_msgs::Int32::ConstPtr &state)
{
	motor_state = state->data;
}

/**
 * Init gripper publisher, set joints etc.
 */
void GripperPublisher::init()
{
	// subscribe to sensor tosensor_valupic
	sens_ = nh_.subscribe(Topic::IR_DISTANCE, 1000, &GripperPublisher::sensorCallback, this);
	// publish joints
	joint_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);
	// subscribe to motor sensor
	motor_state_sub_ = nh_.subscribe(Topic::MOTOR, 1, &GripperPublisher::motorCallback, this);

	joint_state_.name.resize(7);
	joint_state_.position.resize(7);
	joint_state_.name[0] = "object_to_base";
	joint_state_.name[1] = "finger1_middle_to_palm";
	joint_state_.name[2] = "finger1_middle_to_finger1_top";
	joint_state_.name[3] = "finger2_middle_to_palm";
	joint_state_.name[4] = "finger2_middle_to_finger2_top";
	joint_state_.name[5] = "finger3_middle_to_palm";
	joint_state_.name[6] = "finger3_middle_to_finger3_top";
}

/**
 * Open the gripper.
 * @return	boolean		true while gripper is opening.
 */
bool GripperPublisher::openGripper()
{
	if(rotation_top > GripperDefines::ROTATION_TOP_MIN) {
		rotation_top -= GripperDefines::ROTATION_TOP_STEP;
	}
	else
	{
		if(rotation > GripperDefines::ROTATION_MIDDLE_MIN)
			rotation -= GripperDefines::ROTATION_MIDDLE_STEP;
		else
			return false;
	}
	return true;		// still opening
}

/**
 * Close the gripper.
 * @return	boolean		true while gripper is closing.
 */
bool GripperPublisher::closeGripper()
{
	if(rotation > GripperDefines::ROTATION_MIDDLE_MAX) {
		if(rotation_top < GripperDefines::ROTATION_TOP_MAX)
			rotation_top += GripperDefines::ROTATION_TOP_STEP;
		else
			return false;	// closed
	}
	else
		rotation += GripperDefines::ROTATION_MIDDLE_STEP;
	return true;		// still closing
}

/**
 * Update state values.
 */
void GripperPublisher::spin()
{
	ros::Rate loop_rate(30);
	bool current_state = GripperDefines::OPEN;
	while(ros::ok())
	{
		joint_state_.header.stamp = ros::Time::now();
		joint_state_.position[0] = distance;
		joint_state_.position[1] = rotation;
		joint_state_.position[2] = rotation_top;
		joint_state_.position[3] = rotation;
		joint_state_.position[4] = rotation_top;
		joint_state_.position[5] = -rotation;
		joint_state_.position[6] = -rotation_top;

		// check if gripper needs to be closed
		if(current_state != motor_state)
		{
			if(motor_state == GripperDefines::CLOSED)
			{
				if(!closeGripper())
					current_state = motor_state;
			}
			else
				if(!openGripper())
					current_state = motor_state;
		}

		// update values
		joint_.publish(joint_state_);
		ros::spinOnce();
		loop_rate.sleep();
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "gripper_publisher");	// node name gripper_publisher

	GripperPublisher publisher;
	publisher.init();
	publisher.spin();
	return 0;
}
